package messaging;

import java.util.Timer;
import java.util.TimerTask;

import messagingobjects.Point;
import messagingobjects.PoseStamped;
import ros.NodeHandle;
import ros.Publisher;
import ros.Ros;
import ros.RosException;
import ros.Subscriber;
import routingAlgorithms.RoutingAlgorithm;
import dataobjects.Edge;
import dataobjects.Position;
import exceptions.ImpossibleException;

//FIXME add in messages for detecting IEDs and floor obstacles

public class MessagingServices {
	private Ros ros;
	private NodeHandle node;
	private RoutingAlgorithm routing;
	private static long TIMEOUT  = 10000; //10 seconds

	public MessagingServices(RoutingAlgorithm routing){
		ros = Ros.getInstance();
		ros.init("pcp");
		node = ros.createNodeHandle();
		this.routing = routing;
	}

	public void sendMessage(int id, int x, int y){

		Point msg = new Point();
		msg.x = x;
		msg.y = y;
		msg.z = 0;

		String channel = "hvaGoto";
		if (id == 0){
			channel = "seeker1goto";
		} else if (id == 1){
			channel = "seeker2goto";
		} else if (id == 2){
			channel = "seeker3goto";
		}
		// publish the message
		Publisher<Point> pub = null;
		try {
			pub = node.advertise(channel, new Point(), 10);
			pub.publish(msg);
		} catch (RosException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}

	public void run(){
		//TODO set up channels for vision and sonar
		TFCallback callback1 = new TFCallback(0);   
		TFCallback callback2 = new TFCallback(1);   
		TFCallback callback3 = new TFCallback(2);   
		TFCallback callback4 = new TFCallback(-1);  
		IedCallback iedCallback1 = new IedCallback(0);
		SonarCallback sonarCallback1 = new SonarCallback(0);
		ObstacleCallback obstacleCallback1 = new ObstacleCallback(0);
		IedCallback iedCallback2 = new IedCallback(1);
		SonarCallback sonarCallback2 = new SonarCallback(1);
		ObstacleCallback obstacleCallback2 = new ObstacleCallback(1);
		IedCallback iedCallback3 = new IedCallback(2);
		SonarCallback sonarCallback3 = new SonarCallback(2);
		ObstacleCallback obstacleCallback3 = new ObstacleCallback(2);

		try {
			node.subscribe("seeker1Heartbeat", new PoseStamped(), callback1, 10);
			node.subscribe("seeker2Heartbeat", new PoseStamped(), callback2, 10);
			node.subscribe("seeker3Heartbeat", new PoseStamped(), callback3, 10);
			node.subscribe("hvaHeartbeat", new PoseStamped(), callback4, 10);
			node.subscribe("ied1", new Point(), iedCallback1, 10);
			node.subscribe("sonar1", new Point(), sonarCallback1, 10);
			node.subscribe("obstacle1", new Point(), obstacleCallback1, 10);
			node.subscribe("ied2", new Point(), iedCallback2, 10);
			node.subscribe("sonar2", new Point(), sonarCallback2, 10);
			node.subscribe("obstacle2", new Point(), obstacleCallback2, 10);
			node.subscribe("ied3", new Point(), iedCallback3, 10);
			node.subscribe("sonar3", new Point(), sonarCallback3, 10);
			node.subscribe("obstacle3", new Point(), obstacleCallback3, 10);
			node.spin();
		} catch (RosException e) {
			e.printStackTrace();
		}
	}

	/**
	 * Callback class that adds published tf's to the buffer
	 */
	protected class TFCallback implements Subscriber.Callback<PoseStamped> {
		private int id;
		private Timer timer;

		public TFCallback(int id){
			this.id = id;
			timer = new Timer();
		}
		@Override
		public void call(PoseStamped sp) {
			if (sp != null) {
				double x = sp.pose.position.x;
				double y = sp.pose.position.y;
				int xInt = (int) Math.round(x);
				int yInt = (int) Math.round(y);
				System.out.println("Received a position message from seeker "+id+": "+x +" "+y);
				if (Math.abs(x - xInt) < .1 && Math.abs(y - yInt) < .1){
					try {
						System.out.println("Moving to a new position! "+xInt+" "+yInt);
						routing.handlePositionUpdate(id, new Position(xInt, yInt));
					} catch (ImpossibleException e) {
						e.printStackTrace();
					}
				}

				//renew timer
				timer.cancel();
				timer.schedule(new SeekerDied(), TIMEOUT);

			}
		}

		class SeekerDied extends TimerTask {
			public void run() {
				System.out.format("Seeker "+(id+1) +" has died.");
				try {
					routing.handleDeadSeeker(id);
				} catch (ImpossibleException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
				timer.cancel(); //Terminate the timer thread
			}
		}

	}

	/**
	 * Callback class that adds published tf's to the buffer
	 */
	protected class IedCallback implements Subscriber.Callback<Point> {

		private int id;

		public IedCallback(int id){
			this.id = id;
		}

		@Override
		public void call(Point p) {
			if (p != null) {
				double x = p.x;
				double y = p.y;
				System.out.println("Visual IED detection: "+x +" "+y);
				int xInt1 = (int) Math.ceil(x);
				int yInt1 = (int) Math.ceil(y);
				int xInt2 = (int) Math.floor(x);
				int yInt2 = (int) Math.floor(y);
				try {
					routing.addVisualIed(id, new Edge(new Position(xInt1, yInt1), new Position(xInt1, yInt2)), 
							new Edge(new Position(xInt1, yInt2), new Position(xInt2, yInt2)), 
							new Edge(new Position(xInt2, yInt1), new Position(xInt2, yInt2)), 
							new Edge(new Position(xInt1, yInt1), new Position(xInt2, yInt1)));
				} catch (ImpossibleException e) {
					e.printStackTrace();
				}
			}
		}
	}

	/**
	 * Callback class that adds published tf's to the buffer
	 */
	protected class SonarCallback implements Subscriber.Callback<Point> {
		private int id;

		public SonarCallback(int id){
			this.id = id;
		}

		@Override
		public void call(Point p) {
			if (p != null) {
				double x = p.x;
				double y = p.y;
				int xInt = (int) Math.round(x);
				int yInt = (int) Math.round(y);
				System.out.println("Sonar IED detection: "+x +" "+y);
				if (Math.abs(x - xInt) < .1){

					try {
						routing.addSonarIed(id, new Edge(new Position(xInt, (int) Math.floor(y)), new Position(xInt, (int) Math.ceil(y))));
					} catch (ImpossibleException e) {
						e.printStackTrace();
					}
				} else if (Math.abs(y - yInt) < .1){
					try {
						routing.addSonarIed(id, new Edge(new Position(yInt, (int) Math.floor(x)), new Position(yInt, (int) Math.ceil(x))));
					} catch (ImpossibleException e) {
						e.printStackTrace();
					}
				} else {
					System.out.println("Received illegal Sonar IED signal - not on a line!");
				}
			}
		}
	}

	/**
	 * Callback class that adds published tf's to the buffer
	 */
	protected class ObstacleCallback implements Subscriber.Callback<Point> {

		private int id;

		public ObstacleCallback(int id){
			this.id = id;
		}

		@Override
		public void call(Point p) {
			if (p != null) {
				double x = p.x;
				double y = p.y;
				int xInt = (int) Math.round(x);
				int yInt = (int) Math.round(y);
				System.out.println("Tape Obstacle detection: "+x +" "+y);
				if (Math.abs(x - xInt) < .1){

					try {
						routing.addObstacle(id, new Edge(new Position(xInt, (int) Math.floor(y)), new Position(xInt, (int) Math.ceil(y))));
					} catch (ImpossibleException e) {
						e.printStackTrace();
					}
				} else if (Math.abs(y - yInt) < .1){
					try {
						routing.addObstacle(id, new Edge(new Position(yInt, (int) Math.floor(x)), new Position(yInt, (int) Math.ceil(x))));
					} catch (ImpossibleException e) {
						e.printStackTrace();
					}
				} else {
					System.out.println("Received illegal Tape Obstacle - not on a line!");
				}
				//TODO call method
			}
		}
	}
}
